#ifndef __MATHTOOLS_LINE3D_H_INCLUDE__
#define __MATHTOOLS_LINE3D_H_INCLUDE__
#include "mathToolsConfig.h"
#include "vec3f.h"

namespace mt
{
	class line3df
	{
	public:

		//! Default constructor
		/** line from (0,0,0) to (1,1,1) */
		line3df() : start(0, 0, 0), end(1, 1, 1) {}
		//! Constructor with two points
		line3df(float xa, float ya, float za, float xb, float yb, float zb) : start(xa, ya, za), end(xb, yb, zb) {}
		//! Constructor with two points as vectors
		line3df(const vec3f& start, const vec3f& end) : start(start), end(end) {}

		// operators

		line3df operator+(const vec3f& point) const { return line3df (start + point, end + point); }
		line3df & operator+=(const vec3f& point) { start += point; end += point; return *this; }

		line3df operator-(const vec3f& point) const { return line3df (start - point, end - point); }
		line3df & operator-=(const vec3f& point) { start -= point; end -= point; return *this; }

		bool operator==(const line3df & other) const
		{
			return (start == other.start && end == other.end) || (end == other.start && start == other.end);
		}
		bool operator!=(const line3df & other) const
		{
			return !(start == other.start && end == other.end) || (end == other.start && start == other.end);
		}

		// functions
		//! Set this line to a new line going through the two points.
		void setLine(const float& xa, const float& ya, const float& za, const float& xb, const float& yb, const float& zb)
		{
			start.set(xa, ya, za); end.set(xb, yb, zb);
		}
		//! Set this line to a new line going through the two points.
		void setLine(const vec3f& nstart, const vec3f& nend)
		{
			start.set(nstart); end.set(nend);
		}
		//! Set this line to new line given as parameter.
		void setLine(const line3df & line)
		{
			start.set(line.start); end.set(line.end);
		}

		//! Get length of line
		/** \return Length of line. */
		float getLength() const { return start.getDistanceFrom(end); }

		//! Get squared length of line
		/** \return Squared length of line. */
		float getLengthSQ() const { return start.getDistanceFromSQ(end); }

		//! Get middle of line
		/** \return Center of line. */
		vec3f getMiddle() const
		{
			return (start + end) / (float)2;
		}

		//! Get vector of line
		/** \return vector of line. */
		vec3f getVector() const
		{
			return end - start;
		}

		//! Check if the given point is between start and end of the line.
		/** Assumes that the point is already somewhere on the line.
		\param point The point to test.
		\return True if point is on the line between start and end, else false.
		*/
		bool isPointBetweenStartAndEnd(const vec3f& point) const
		{
			return point.isBetweenPoints(start, end);
		}

		//! Get the closest point on this line to a point
		/** \param point The point to compare to.
		\return The nearest point which is part of the line. */
		vec3f getClosestPoint(const vec3f& point) const
		{
			vec3f c = point - start;
			vec3f v = end - start;
			float d = (float)v.getLength();
			v /= d;
			float t = v.dotProduct(c);

			if (t < (float)0.0)
				return start;
			if (t > d)
				return end;

			v *= t;
			return start + v;
		}

		//! Check if the line intersects with a shpere
		/** \param sorigin: Origin of the shpere.
		\param sradius: Radius of the sphere.
		\param outdistance: The distance to the first intersection point.
		\return True if there is an intersection.
		If there is one, the distance to the first intersection point
		is stored in outdistance. */
		bool getIntersectionWithSphere(vec3f sorigin, float sradius, double& outdistance) const
		{
			const vec3f q = sorigin - start;
			float c = q.getLength();
			float v = q.dotProduct(getVector().normalize());
			float d = sradius * sradius - (c*c - v*v);

			if (d < 0.0)
				return false;

			outdistance = v - mt::squareroot(d);
			return true;
		}

		// member variables

		//! Start point of line
		vec3f start;
		//! End point of line
		vec3f end;
	};

}

#endif//